#include <Stage-4.1/stage.hh>
#include <ros/ros.h>
#include <std_msgs/Float32.h>

#include <fstream>
#include <iostream>
#include <stdlib.h>

#include "vrat.h"
#include "ply.h"
#include "3ds.h"
#include "floorplan.h"
#include "light.h"

#include <FL/Fl.H>
#include <FL/Fl_Window.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_File_Chooser.H>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
using boost::property_tree::ptree;

#include <GL/glut.h>

#define SAVE_RATS "Save Rats"
#define SAVE_SCENE "Save Scene"
#define FREEZE "Freeze Scene"
#define UNFREEZE "Unfreeze Scene"
#define ADD_MODEL "Add Model"
#define RELOAD_SCENE "Reload Scene"

void read_scene(std::string fname, Stg::WorldGui *stage, std::vector<Stg::Model*>& models);
void save_scene(std::string fname, std::vector<Stg::Model*>& models);
void add_model(std::string fname, Stg::WorldGui *stage, std::vector<Stg::Model*>& models);
void freeze_scene(std::vector<Stg::Model*>& models);
void unfreeze_scene(std::vector<Stg::Model*>& models);

void read_rats(std::string fname, Stg::WorldGui *stage, std::vector<vRat*>& rats, ptree& vrat_config);
void save_rats(std::string fname, std::vector<vRat*>& rats);
void add_rat(std::string colour, Stg::WorldGui *stage, std::vector<vRat*>& rats, ptree& vrat_config);

void button_cb(Fl_Widget* o, void *data);

void callback_sun(std_msgs::Float32ConstPtr val);

struct CBSTRUCT
{
	std::string rats_fname;
	std::vector<vRat*> *rats;
	std::string models_fname;
	std::vector<Stg::Model*> *models;
	Stg::WorldGui *stage;
};

Light sun;
float sun_radius, sun_z_angle;

int main(int argc, char **argv)
{
	if (argc < 2)
	{
		printf("usage: %s config_file\n", argv[0]);
		return -1;
	}
	ptree config, sim_config, vrat_config;
	try
	{
		read_ini(argv[1], config);
		sim_config = config.get_child("simulator");
		vrat_config = config.get_child("vrat");
	}
	catch (std::exception const& e)
	{
		printf("Unable to open configuration file - %s\n", e.what());
		return -1;
	}

	ros::init(argc, argv, "ratstage");

	// sun
	ros::NodeHandle nh;
	unsigned qsize = 1000;
	sun_radius = sim_config.get<float>("sun_radius");
	sun_z_angle = sim_config.get<float>("sun_z_angle");
	ros::Subscriber sub_sun = nh.subscribe<std_msgs::Float32>(sim_config.get<std::string>("sun_topic"), qsize, callback_sun);
	sun.x = sun.y = 0;
	sun.z = sun_radius;

	Stg::Init(&argc, &argv);

	Stg::WorldGui stage(640, 480, "ratstage");

	std::vector<vRat*> rats;
	std::vector<Stg::Model*> models;

	// do not change this order
	read_rats(sim_config.get<std::string>("rats_file"), &stage, rats, vrat_config);
	Floorplan fp(&stage, sim_config.get<std::string>("floorplan_file"),
			sim_config.get<float>("floorplan_width"),
			sim_config.get<float>("floorplan_height"),
			sim_config.get<float>("floorplan_wall_height"));
	stage.Load(sim_config.get<std::string>("stage_file"));
	read_scene(sim_config.get<std::string>("scene_file"), &stage, models);

	ros::WallRate r(100.0);
	stage.Start();

	Fl_Window win(200, 600, "Simulator Configurator");
	win.begin();
	Fl_Button b_rsave(0, 0, 200, 25, SAVE_RATS);
	Fl_Button b_msave(0, 30, 200, 25, SAVE_SCENE);
	Fl_Button b_freeze(0, 60, 200, 25, UNFREEZE);
	Fl_Button b_madd(0, 90, 200, 25, ADD_MODEL);
	Fl_Button b_mreload(0, 120, 200, 25, RELOAD_SCENE);
	win.end();
	CBSTRUCT cbs = { sim_config.get<std::string>("rats_file"), &rats, sim_config.get<std::string>("scene_file"), &models, &stage };
	b_rsave.callback(button_cb, &cbs);
	b_msave.callback(button_cb, &cbs);
	b_freeze.callback(button_cb, &cbs);
	b_madd.callback(button_cb, &cbs);
	b_mreload.callback(button_cb, &cbs);
	win.show();

	while(!stage.TestQuit())
	{
		Fl::wait(r.expectedCycleTime().toSec());
		ros::spinOnce();
	}

	return 0;
}

void callback_sun(std_msgs::Float32ConstPtr val)
{
	float ang = val->data * M_PI/180.0;
	sun.x = sun_radius*cos(sun_z_angle * M_PI/180.0)*sin(ang);
	sun.y = sun_radius*sin(sun_z_angle * M_PI/180.0)*cos(ang);
	sun.z = sun_radius*cos(ang);
}

void read_scene(std::string fname, Stg::WorldGui *stage, std::vector<Stg::Model*>& models)
{
	std::fstream file(fname.c_str());
	if (!file.is_open())
	{
		printf("read_scene(): could not open %s\n", fname.c_str());
		return;
	}

	std::string line;

	char model_name[100];
	char number[1000];
	float scale, x, y, z, angle;

	unsigned i = 0;
	while (file.good())
	{
		getline(file, line);
		if (file.eof()) break;
		if (line[0] == '#' || line == "") continue;
		sscanf(line.c_str(), "%s %f %f %f %f %f", model_name, &x, &y, &z, &angle, &scale);

		std::string str_name(model_name);
		if (str_name.find(".ply") != std::string::npos || str_name.find(".PLY") != std::string::npos)
		{
			ModelPLY *model = new ModelPLY(stage, Stg::Pose(x, y, z, angle*M_PI/180.0), str_name);
			model->SetGuiMove(false);
			model->SetRangerReturn(0.0);
			model->SetObstacleReturn(false);
			models.push_back(model);
			std::string scalestr = "scale";
			model->SetProperty(scalestr, (void*)&(model->scale));
			model->scale = scale;
			std::string visible = "visible";
			model->SetProperty(visible, (void*)1);
		}
		else if (str_name.find(".3ds") != std::string::npos || str_name.find(".3DS") != std::string::npos)
		{
			Model3DS *model = new Model3DS(stage, Stg::Pose(x, y, z, angle*M_PI/180.0), str_name);
			model->SetGuiMove(false);
			model->SetRangerReturn(0.0);
			model->SetObstacleReturn(false);
			models.push_back(model);
			std::string scalestr = "scale";
			model->SetProperty(scalestr, (void*)&(model->scale));
			model->scale = scale;
			std::string visible = "visible";
			model->SetProperty(visible, (void*)1);
		}
		sprintf(number, "%d", i++);
	}
	file.close();
	printf("read scene file '%s'\n", fname.c_str());
}

void read_rats(std::string fname, Stg::WorldGui *stage, std::vector<vRat*>& rats, ptree& vrat_config)
{
	std::fstream file(fname.c_str());
	if (!file.is_open())
	{
		printf("read_rats(): could not open %s\n", fname.c_str());
		return;
	}

	std::string line;

	char colour[100];
	float x, y, angle;

	while (file.good())
	{
		getline(file, line);
		if (line[0] == '#' || line == "") continue;
		sscanf(line.c_str(), "%s %f %f %f", colour, &x, &y, &angle);
		std::string c = colour;
		vRat *rat = new vRat(stage, c, vrat_config);
		rat->SetPose(Stg::Pose(x, y, 0, angle*M_PI/180.0));

		if (sun_radius == 0.0) rat->sun = NULL;
		else rat->sun = &sun;
		rats.push_back(rat);
		printf("read_rats(): added %s rat at (%f, %f) at %f degrees and/or radians\n", colour, x, y, angle);
		if (file.eof()) break;
	}
	file.close();
}

void save_scene(std::string fname, std::vector<Stg::Model*>& models)
{
	std::fstream file(fname.c_str());
	if (!file.is_open())
	{
		printf("save_scene(): could not open %s\n", fname.c_str());
		return;
	}

	char buf[200];

	file << "# filename x y z ang scale\n";


	std::string scale("scale");
	std::string visible("visible");

	for (unsigned i = 0; i < models.size(); i++)
	{
		if (!(bool)(models[i]->GetProperty(visible))) continue;
		Stg::Pose p = models[i]->GetGlobalPose();
		sprintf(buf, "%s %f %f %f %f %f", models[i]->GetModelType().c_str(), p.x, p.y, p.z, p.a*180.0/M_PI, *(float*)(models[i]->GetProperty(scale)) );
		file << std::string(buf) << "\n";
	}

	file << "\n";

	printf("Scene saved to '%s'\n", fname.c_str());
}

void save_rats(std::string fname, std::vector<vRat*>& rats)
{
	std::fstream file(fname.c_str());
	if (!file.is_open())
	{
		printf("save_rats(): could not open %s\n", fname.c_str());
		return;
	}

	char buf[200];

	file << "# colour x y ang\n";

	for (unsigned i = 0; i < rats.size(); i++)
	{
		Stg::Pose p = rats[i]->GetGlobalPose();
		sprintf(buf, "%s %f %f %f ", rats[i]->GetModelType().c_str(), p.x, p.y, p.a*180.0/M_PI);
		file << std::string(buf) << "\n";
	}

	file << "\n";

	printf("Rats saved to '%s'\n", fname.c_str());
}

void freeze_scene(std::vector<Stg::Model*>& models)
{
	printf("Scene frozen.\n");
	for (unsigned i = 0; i < models.size(); i++)
		models[i]->SetGuiMove(false);
}

void unfreeze_scene(std::vector<Stg::Model*>& models)
{
	printf("Scene unfrozen.\n");
	for (unsigned i = 0; i < models.size(); i++)
			models[i]->SetGuiMove(true);
}

void button_cb(Fl_Widget* o, void *data)
{
	Fl_Button *b = (Fl_Button*)o;
	CBSTRUCT *cbs = (CBSTRUCT*)data;

	std::string label = b->label();
	if (label == SAVE_RATS)
	{
		save_rats(cbs->rats_fname, *(cbs->rats));
	}
	else if (label == SAVE_SCENE)
	{
		save_scene(cbs->models_fname, *(cbs->models));
	}
	else if (label == FREEZE)
	{
		freeze_scene(*(cbs->models));
		b->label(UNFREEZE);
	}
	else if (label == UNFREEZE)
	{
		unfreeze_scene(*(cbs->models));
		b->label(FREEZE);
	}
	else if (label == ADD_MODEL)
	{
		char* fname = fl_file_chooser("Choose .ply or .3ds model file", "", "", 1);
		if (fname == NULL) return;
		add_model(std::string(fname), cbs->stage, *(cbs->models));
	}
	else if (label == RELOAD_SCENE)
	{
		std::string visible = "visible";
		for (unsigned i = 0; i < cbs->models->size(); i++)
		{
			(*(cbs->models))[i]->SetProperty(visible, (void*)0);
			(*(cbs->models))[i]->SetGuiMove(false);
		}
		cbs->models->clear();
		read_scene(cbs->models_fname, cbs->stage, *(cbs->models));
	}

}

void add_model(std::string fname, Stg::WorldGui *stage, std::vector<Stg::Model*>& models)
{
	if (fname.find(".ply") != std::string::npos || fname.find(".PLY") != std::string::npos)
	{
		ModelPLY *model = new ModelPLY(stage, Stg::Pose(0, 0, 0, 0), fname);
		model->SetGuiMove(false);
		model->SetRangerReturn(0.0);
		model->SetObstacleReturn(false);
		models.push_back(model);
		std::string scalestr = "scale";
		model->SetProperty(scalestr, (void*)&(model->scale));
		std::string visible = "visible";
		model->SetProperty(visible, (void*)1);
		model->scale = 1;
	}
	else if (fname.find(".3ds") != std::string::npos || fname.find(".3DS") != std::string::npos)
	{
		Model3DS *model = new Model3DS(stage, Stg::Pose(0, 0, 0, 0), fname);
		model->SetGuiMove(false);
		model->SetRangerReturn(0.0);
		model->SetObstacleReturn(false);
		models.push_back(model);
		std::string scalestr = "scale";
		model->SetProperty(scalestr, (void*)&(model->scale));
		std::string visible = "visible";
		model->SetProperty(visible, (void*)1);
		model->scale = 1;
	}
}
